/* copyright (c) 2007 magnus auvinen, see licence.txt for more info */
#include <base/system.h>
#include <base/math.hpp>
#include <base/vmath.hpp>

#include <math.h>
#include <engine/e_common_interface.h>
//#include <game/mapitems.hpp>
#include <game/layers.hpp>
#include <game/collision.hpp>

#include <vector>


static TILE *tiles;
static std::vector<QUAD_PTR*> quads;//I, jacob added this to handle collisions with the collideable quads in the map
static int width = 0;
static int height = 0;

int col_width() { return width; }
int col_height() { return height; }

float AngleBetween(vec2 vect1, vec2 vect2)
{
   
  float angle1 = -(180/pi) * atan2(vect1.x*vect2.y - vect1.y*vect2.x, vect1.x*vect2.x+vect1.y*vect2.y);
  
  return angle1;
}
bool IntersectionOfTwoLines(const vec2& a,const vec2& b, POINT pc, POINT pd, vec2 &result)                                
{
	pc.x >>=10;
	pc.y >>=10;
	pd.x >>=10;
	pd.y >>=10;

	vec2 c = vec2(pc.x, pc.y); 
	vec2 d = vec2(pd.x, pd.y);

    float r, s;

    float denominator = (b.x - a.x) * (d.y - c.y) - (b.y - a.y) * (d.x - c.x);

    // If the denominator in above is zero, AB & CD are colinear
    if (denominator == 0)
        return false;

    float numeratorR = (a.y - c.y) * (d.x - c.x) - (a.x - c.x) * (d.y - c.y);
    //  If the numerator above is also zero, AB & CD are collinear.
    //  If they are collinear, then the segments may be projected to the x- 
    //  or y-axis, and overlap of the projected intervals checked.

    r = numeratorR / denominator;

    float numeratorS = (a.y - c.y) * (b.x - a.x) - (a.x - c.x) * (b.y - a.y);

    s = numeratorS / denominator;

    //  If 0<=r<=1 & 0<=s<=1, intersection exists
    //  r<0 or r>1 or s<0 or s>1 line segments do not intersect
    if (r < 0 || r > 1 || s < 0 || s > 1)
        return false;

    ///*
    //    Note:
    //    If the intersection point of the 2 lines are needed (lines in this
    //    context mean infinite lines) regardless whether the two line
    //    segments intersect, then
    //
    //        If r>1, P is located on extension of AB
    //        If r<0, P is located on extension of BA
    //        If s>1, P is located on extension of CD
    //        If s<0, P is located on extension of DC
    //*/

    // Find intersection point
    result.x = (float)(a.x + (r * (b.x - a.x)));
    result.y = (float)(a.y + (r * (b.y - a.y)));

    return true;
}

int col_init()
{
	width = layers_game_layer()->width;
	height = layers_game_layer()->height;
	tiles = (TILE *)map_get_data(layers_game_layer()->data);

	////jacob added alll this, loads in the quads from the map file into the vector array quads \/ \/ \/
	
	while (quads.size() > 0) //this ensures that when u quit and rejoin a server, it doesnt add the same quads to the vector
	{
		quads.pop_back();
	}

	std::vector<MAPITEM_LAYER_QUADS*> game_quad_layers_temp = layers_game_quad_layers(); //a temp 

	//for (std::vector<MAPITEM_LAYER_QUADS*>::iterator it = layers_game_quad_layers().begin(); it != layers_game_quad_layers().end(); it++)
	for (std::vector<MAPITEM_LAYER_QUADS*>::iterator it = game_quad_layers_temp.begin(); it != game_quad_layers_temp.end(); it++)
	{
		
		quads.push_back(new QUAD_PTR((QUAD *)map_get_data((*it)->data), (*it)->num_quads));

		std::vector<QUAD_PTR*>::iterator quadptr = quads.begin();

		for (int i =0; i < (*quadptr)->num_quads; i++)
		{
			if((*quadptr)->quad[i].index == QUAD_DEATH)
				(*quadptr)->quad[i].index = COLFLAG_DEATH;
			else if((*quadptr)->quad[i].index == QUAD_SOLID)
				(*quadptr)->quad[i].index = COLFLAG_SOLID;
			else if((*quadptr)->quad[i].index == QUAD_NOHOOK)
				(*quadptr)->quad[i].index = COLFLAG_SOLID|COLFLAG_NOHOOK;
			else
				(*quadptr)->quad[i].index = 0;
		}
	}
	
	for(int i = 0; i < width*height; i++)
	{
		int index = tiles[i].index;
		
		if(index > 128)
			continue;
		
		if(index == TILE_DEATH)
			tiles[i].index = COLFLAG_DEATH;
		else if(index == TILE_SOLID)
			tiles[i].index = COLFLAG_SOLID;
		else if(index == TILE_NOHOOK)
			tiles[i].index = COLFLAG_SOLID|COLFLAG_NOHOOK;
		else
			tiles[i].index = 0;
	}
				
	return 1;
}


int col_get(int x, int y)
{
	int nx = clamp(x/32, 0, width-1);
	int ny = clamp(y/32, 0, height-1);
	
	if(tiles[ny*width+nx].index > 128)
		return 0;
	return tiles[ny*width+nx].index;
}

int col_is_solid(int x, int y)
{
	return col_get(x,y)&COLFLAG_SOLID;
}


// TODO: rewrite this smarter!
int col_intersect_line(vec2 pos0, vec2 pos1, vec2 *out_collision, vec2 *out_before_collision)
{
	float d = distance(pos0, pos1);
	vec2 last = pos0;
	
	for(float f = 0; f < d; f++)
	{
		float a = f/d;
		vec2 pos = mix(pos0, pos1, a);
		if(col_is_solid(round(pos.x), round(pos.y)))
		{
			if(out_collision)
				*out_collision = pos;
			if(out_before_collision)
			{
				*out_before_collision = last;
			}
			return col_get(round(pos.x), round(pos.y));
		}
		last = pos;
	}
	if(out_collision)
		*out_collision = pos1;
	if(out_before_collision)
		*out_before_collision = pos1;
	return 0;
}

//Jacob: projects 2 quads onto one separating axis, and returns true if the 2 quads overlap on all cases.
bool col_projection_check(MOVING_QUAD quad1, QUAD quad2, const vec2& separatingAxis)
{
    float quad1_Min, quad1_Max;
	float quad2_Min, quad2_Max;
	float dotProdCheck;

	for (int i =0; i < 5; i++)
	{
		//shift the points to be normal for collision, so its not fucking fucked
		//quad1.points[i].x >>= 10;
		//quad1.points[i].y >>= 10; 
		quad2.points[i].x >>= 10;
		quad2.points[i].y >>= 10;
	}

    quad1_Min = quad1_Max = dot(vec2(quad1.points[0].x, quad1.points[0].y), separatingAxis);

	quad2_Min = quad2_Max = dot(vec2(quad2.points[0].x, quad2.points[0].y), separatingAxis);


	for (int i = 1; i < 4; i++)
	{
		//project quad1 onto the separating axis, defined by the normal of a side of one of the 2 quads.
		dotProdCheck = dot(vec2(quad1.points[i].x, quad1.points[i].y), separatingAxis);

		if (dotProdCheck > quad1_Max)
			quad1_Max = dotProdCheck;
		if (dotProdCheck < quad1_Min)
			quad1_Min = dotProdCheck;

		////...now project quad2 onto the separating axis.
		dotProdCheck = dot(vec2(quad2.points[i].x, quad2.points[i].y), separatingAxis);

		if (dotProdCheck > quad2_Max)
			quad2_Max = dotProdCheck;
		if (dotProdCheck < quad2_Min)
			quad2_Min = dotProdCheck;
	}

    //now check to see if the two quads overlap eachother. if they dont, return false.
    if (quad1_Max < quad2_Min)
        return false;
    if (quad1_Min > quad2_Max)
        return false;

    return true;
}
//separating axis check on 2 quads.
bool col_separating_axis_check(const MOVING_QUAD& quad1, const QUAD& quad2)
{
	vec2 separatingAxis;
	for (int i = 0; i < 4; i++)
	{
		separatingAxis = quad1.sidenormals[i];
		if(!col_projection_check(quad1, quad2, separatingAxis))
			return false;

		separatingAxis = quad2.sidenormals[i];
		if (!col_projection_check(quad1, quad2, separatingAxis))
			return false;
	}
	for (int i = 0; i < 4; i++)
	{
		for (int j = 0; j < 4; j++)
		{

		}
	}
	//if each axis finds an overlap (and doesnt return false), return true.
	return true;
}


//does the check between a quad and all the map quads. Finds the overlap at which the moving quad should be displaced as well.
//also tells if we are on the ground, agaistn a wall 
bool col_quads_check(const MOVING_QUAD& quad1, vec2 &overlap, bool &isonground)
{
	bool returnvalue = false;
	std::vector<QUAD*> collList;
	for (std::vector<QUAD_PTR*>::iterator quadptr = quads.begin(); quadptr != quads.end(); quadptr++)
	{
		for (int i =0; i < (*quadptr)->num_quads; i++)
		{
			if((*quadptr)->quad[i].index == COLFLAG_SOLID)
			{
				if (col_separating_axis_check( quad1,(*quadptr)->quad[i] ))
				{
					returnvalue = true;

					collList.push_back(&(*quadptr)->quad[i]);
					//return true;
				}
			}
		}
	}
	
	//find the overlaps of all quads collided with.
	if (collList.size() > 0)
	{
		for (std::vector<QUAD*>::iterator collListptr = collList.begin(); collListptr != collList.end(); collListptr++)
		{
			vec2 tempOverlap = vec2(1000,1000);
			FindOverlap(quad1, *(*collListptr), tempOverlap);

			float angle = AngleBetween(tempOverlap, vec2(0,1));
			

			if (65 > angle && angle > -65)
            {
				if (tempOverlap.y == 0 && tempOverlap.x ==0) //dealing with a special case of a horizontal ceiling line
				{
					isonground = false;
				}
				else
				{
					isonground = true;
					//dbg_msg("jacobs msg: ", "angle = %f, overlap(%f, %f)", angle, tempOverlap.x, tempOverlap.y);
				}
			}

			overlap += tempOverlap;

		}
	}

	//return false;
	return returnvalue;
}

//Jacob: projects 1 quad and 1 point onto one separating axis, and returns true if they overlap on all separating axes.
bool col_projection_check(QUAD quad1, vec2 point, const vec2& separatingAxis)
{
	for (int i =0; i < 5; i++)
	{
		quad1.points[i].x >>= 10;
		quad1.points[i].y >>= 10;
	}
	
    float quad1_Min, quad1_Max;
	float point_Min, point_Max;
	float dotProdCheck;

    quad1_Min = quad1_Max = dot(vec2(quad1.points[0].x, quad1.points[0].y), separatingAxis);


	for (int i = 1; i < 4; i++)
	{
		//project quad1 onto the separating axis, defined by the normal of a side of one of the 2 quads.
		dotProdCheck = dot(vec2(quad1.points[i].x, quad1.points[i].y), separatingAxis);

		if (dotProdCheck > quad1_Max)
			quad1_Max = dotProdCheck;
		if (dotProdCheck < quad1_Min)
			quad1_Min = dotProdCheck;
		
	}

	////...now project the point onto the separating axis.
		point_Max = point_Min = dot(point, separatingAxis);

		/*if (dotProdCheck > point_Max)
			point_Max = dotProdCheck;
		if (dotProdCheck < point_Min)
			point_Min = dotProdCheck;*/

    //now check to see if the two quads overlap eachother. if they dont, return false.
    if (quad1_Max < point_Min)
        return false;
    if (quad1_Min > point_Max)
        return false;

    return true;
}


//separating axis check on 1 quad and 1 point.
bool col_separating_axis_check(const QUAD& quad1, const vec2& point)
{
	vec2 separatingAxis;
	for (int i = 0; i < 4; i++)
	{
		//dbg_msg("jacobs dbg msg: ", "quad1[i].x = %i", quad1.points[i].x);
		separatingAxis = quad1.sidenormals[i];
		if(!col_projection_check(quad1, point, separatingAxis))
			return false;

	}
	//if each axis finds an overlap (and doesnt return false), return true.
	return true;
}


//does the check between a point and all the quads
bool col_quads_point_check(vec2 point)
{
	//programm ISSS getting here

	for (std::vector<QUAD_PTR*>::iterator quadptr = quads.begin(); quadptr != quads.end(); quadptr++)
	{
		for (int i =0; i < (*quadptr)->num_quads; i++)
		{
			if((*quadptr)->quad[i].index == COLFLAG_SOLID)
			{
				if(col_separating_axis_check((*quadptr)->quad[i], point))
					return true;
			}
		}
	}

	return false;
}



//Jacob: projects 1 quads and 1 line onto one separating axis, and returns true if they overlap on all cases.
bool col_projection_check(QUAD quad1, const vec2& point1, const vec2& point2, const vec2& separatingAxis)
{
    float quad1_Min, quad1_Max;
	float quad2_Min, quad2_Max;
	float dotProdCheck;

	for (int i =0; i < 5; i++)
	{
		//shift the points to be normal for collision, so its not fucking fucked
		quad1.points[i].x >>= 10;
		quad1.points[i].y >>= 10;


	}
    quad1_Min = quad1_Max = dot(vec2(quad1.points[0].x, quad1.points[0].y), separatingAxis);


	for (int i = 1; i < 4; i++)
	{
		//project quad1 onto the separating axis, defined by the normal of a side of one of the 2 quads.
		dotProdCheck = dot(vec2(quad1.points[i].x, quad1.points[i].y), separatingAxis);

		if (dotProdCheck > quad1_Max)
			quad1_Max = dotProdCheck;
		if (dotProdCheck < quad1_Min)
			quad1_Min = dotProdCheck;
	}
	
	quad2_Min = quad2_Max = dot(point1, separatingAxis);
	////...now project quad2 onto the separating axis.

	dotProdCheck = dot(point2, separatingAxis);

	if (dotProdCheck > quad2_Max)
		quad2_Max = dotProdCheck;
	if (dotProdCheck < quad2_Min)
		quad2_Min = dotProdCheck;
	

    //now check to see if the two quads overlap eachother. if they dont, return false.
    if (quad1_Max < quad2_Min)
	{
		dbg_msg("Jacobs dbg msg: ", "return false");
        return false;
	}
	if (quad1_Min > quad2_Max)
	{    
		dbg_msg("Jacobs dbg msg: ", "return false");
		return false;
	}
	dbg_msg("Jacobs dbg msg: ", "return true");
    return true;
}
//separating axis check on 1 quad and 1 line.
bool col_separating_axis_check(const QUAD& quad1, const vec2& point1, const vec2& point2)
{
	vec2 separatingAxis;
	for (int i = 0; i < 4; i++)
	{
		separatingAxis = quad1.sidenormals[i];
		if(!col_projection_check(quad1, point1, point2, separatingAxis))
		{
			//dbg_msg("Jacobs dbg msg: ", "return false");
			return false;
		}
	}

	separatingAxis = normal_of(point1, point2);
	if (!col_projection_check(quad1, point1, point2, separatingAxis))
	{
		return false;
	}
	//if each axis finds an overlap (and doesnt return false), return true.
	return true;
}

//does the check between a line and all the quads. finds the intersection and point of intersection closest to point1
bool col_quads_line_check(const vec2 &point1, const vec2 &point2, vec2 &pointOfIntersect)
{
	vec2 tempPointOfIntersect;
	bool returnvalue = false;

	for (std::vector<QUAD_PTR*>::iterator quadptr = quads.begin(); quadptr != quads.end(); quadptr++)
	{
		for (int i =0; i < (*quadptr)->num_quads; i++)
		{
			if((*quadptr)->quad[i].index == COLFLAG_SOLID)
			{
				if (IntersectionOfTwoLines( point1, point2, (*quadptr)->quad[i].points[0], (*quadptr)->quad[i].points[1], tempPointOfIntersect ))
				{
					returnvalue = true;
					if (length(tempPointOfIntersect - point1) < length(pointOfIntersect - point1))
					{
						pointOfIntersect = tempPointOfIntersect;
					}
				}	
				if (IntersectionOfTwoLines( point1, point2, (*quadptr)->quad[i].points[1], (*quadptr)->quad[i].points[3], tempPointOfIntersect ))
				{
					returnvalue = true;
					if (length(tempPointOfIntersect - point1) < length(pointOfIntersect - point1))
					{
						pointOfIntersect = tempPointOfIntersect;
					}
				}
				if (IntersectionOfTwoLines( point1, point2, (*quadptr)->quad[i].points[3], (*quadptr)->quad[i].points[2], tempPointOfIntersect ))
				{
					returnvalue = true;
					if (length(tempPointOfIntersect - point1) < length(pointOfIntersect - point1))
					{
						pointOfIntersect = tempPointOfIntersect;
					}
				}
				if (IntersectionOfTwoLines( point1, point2, (*quadptr)->quad[i].points[2], (*quadptr)->quad[i].points[0], tempPointOfIntersect ))
				{
					returnvalue = true;
					if (length(tempPointOfIntersect - point1) < length(pointOfIntersect - point1))
					{
						pointOfIntersect = tempPointOfIntersect;
					}
				}
			}
		}
	}

	if (returnvalue)
		return true;
	
	return false;
}



bool col_quads_line_check(const vec2 &point1, const vec2 &point2, vec2 &pointOfIntersect, vec2 &angleOfReflect)
{
	vec2 tempPointOfIntersect;
	bool returnvalue = false;

	for (std::vector<QUAD_PTR*>::iterator quadptr = quads.begin(); quadptr != quads.end(); quadptr++)
	{
		for (int i =0; i < (*quadptr)->num_quads; i++)
		{
			if((*quadptr)->quad[i].index == COLFLAG_SOLID)
			{
				if (IntersectionOfTwoLines( point1, point2, (*quadptr)->quad[i].points[0], (*quadptr)->quad[i].points[1], tempPointOfIntersect ))
				{
					returnvalue = true;
					if (length(tempPointOfIntersect - point1) < length(pointOfIntersect - point1))
					{
						pointOfIntersect = tempPointOfIntersect;
						angleOfReflect = (*quadptr)->quad[i].sidenormals[0];
					}
				}	
				if (IntersectionOfTwoLines( point1, point2, (*quadptr)->quad[i].points[1], (*quadptr)->quad[i].points[3], tempPointOfIntersect ))
				{
					returnvalue = true;
					if (length(tempPointOfIntersect - point1) < length(pointOfIntersect - point1))
					{
						pointOfIntersect = tempPointOfIntersect;
						angleOfReflect = (*quadptr)->quad[i].sidenormals[1];
					}
				}
				if (IntersectionOfTwoLines( point1, point2, (*quadptr)->quad[i].points[3], (*quadptr)->quad[i].points[2], tempPointOfIntersect ))
				{
					returnvalue = true;
					if (length(tempPointOfIntersect - point1) < length(pointOfIntersect - point1))
					{
						pointOfIntersect = tempPointOfIntersect;
						angleOfReflect = (*quadptr)->quad[i].sidenormals[2];
					}
				}
				if (IntersectionOfTwoLines( point1, point2, (*quadptr)->quad[i].points[2], (*quadptr)->quad[i].points[0], tempPointOfIntersect ))
				{
					returnvalue = true;
					if (length(tempPointOfIntersect - point1) < length(pointOfIntersect - point1))
					{
						pointOfIntersect = tempPointOfIntersect;
						angleOfReflect = (*quadptr)->quad[i].sidenormals[3];
					}
				}
			}
		}
	}

	if (returnvalue)
		return true;
	
	return false;
}


void CalculateOverlap(const MOVING_QUAD &quad1, QUAD quad2, const vec2 &separatingAxis, vec2& minimumOverlap)
{

    /*vec2 minimumOverlap = vec2(1000,1000);*/

	float quad1_Min, quad1_Max;
	float quad2_Min, quad2_Max;
	float dotProdCheck;

	for (int i =0; i < 5; i++)
	{
		//shift the points to be normal for collision, so its not fucking fucked
		//quad1.points[i].x >>= 10;
		//quad1.points[i].y >>= 10; 
		quad2.points[i].x >>= 10;
		quad2.points[i].y >>= 10;
	}

    quad1_Min = quad1_Max = dot(vec2(quad1.points[0].x, quad1.points[0].y), separatingAxis);

	quad2_Min = quad2_Max = dot(vec2(quad2.points[0].x, quad2.points[0].y), separatingAxis);


	for (int i = 1; i < 4; i++)
	{
		//project quad1 onto the separating axis, defined by the normal of a side of one of the 2 quads.
		dotProdCheck = dot(vec2(quad1.points[i].x, quad1.points[i].y), separatingAxis);

		if (dotProdCheck > quad1_Max)
			quad1_Max = dotProdCheck;
		if (dotProdCheck < quad1_Min)
			quad1_Min = dotProdCheck;

		////...now project quad2 onto the separating axis.
		dotProdCheck = dot(vec2(quad2.points[i].x, quad2.points[i].y), separatingAxis);

		if (dotProdCheck > quad2_Max)
			quad2_Max = dotProdCheck;
		if (dotProdCheck < quad2_Min)
			quad2_Min = dotProdCheck;
	}

	//-----------HANDLE THE OVERLAP CASE BY SETTING THE MINIMUM OVERLAP

    //if the max of this quad is overlaping
    if (quad1_Min <= quad2_Min && quad1_Max <= quad2_Max && quad1_Max >= quad2_Min)
    {
        vec2 projectionOverlap = (normalize(separatingAxis)) * (quad1_Max - quad2_Min);
        
        if (length(minimumOverlap) > length(projectionOverlap))
            minimumOverlap = projectionOverlap;
    }
    //if the min of this quad is overlapping
	else if (quad1_Min <= quad2_Max && quad1_Min >= quad2_Min && quad1_Max >= quad2_Max)
    {
        vec2 projectionOverlap = (normalize(separatingAxis)) * (quad1_Min - quad2_Max);

        if (length(minimumOverlap) > length(projectionOverlap))
            minimumOverlap = projectionOverlap;
    }
    //if the other quad is contained within this one
    else if (quad1_Min <= quad2_Min && quad1_Max >= quad2_Max)
    {
        vec2 projectionOverlap = (normalize(separatingAxis)) * (quad1_Min - quad2_Max);
        if (length(minimumOverlap) > length(projectionOverlap))
            minimumOverlap = projectionOverlap;

         projectionOverlap = (normalize(separatingAxis)) * (quad1_Max - quad2_Min);
        if (length(minimumOverlap) > length(projectionOverlap))
            minimumOverlap = projectionOverlap;
    }
    //if this quad is contained within the other one
    else if (quad1_Min >= quad2_Min && quad1_Max <= quad2_Max)
    {
        vec2 projectionOverlap = (normalize(separatingAxis)) * (quad1_Min - quad2_Max);
        if (length(minimumOverlap) > length(projectionOverlap))
            minimumOverlap = projectionOverlap;

         projectionOverlap = (normalize(separatingAxis)) * (quad1_Max - quad2_Min);
        if (length(minimumOverlap) > length(projectionOverlap))
            minimumOverlap = projectionOverlap;
    }
	else
	{
		dbg_msg("jacobs msg", "\nq1Min = %f,\nq1Max = %f,\nq2Min = %f,\nq2Max = %f", quad1_Min, quad1_Max, quad2_Min, quad2_Max);
		dbg_msg("jacobs msg", "other case??");
	}
	/*if (length(overlap) > length(minimumOverlap))
		overlap = minimumOverlap;*/
}


void FindOverlap(const MOVING_QUAD &quad1, QUAD quad2, vec2& overlap)
{
	vec2 separatingAxis;
	for (int i = 0; i < 4; i++)
	{
		
		separatingAxis = quad2.sidenormals[i];
		CalculateOverlap(quad1, quad2, separatingAxis, overlap);

		//separatingAxis = quad1.sidenormals[i];
		//CalculateOverlap(quad1, quad2, separatingAxis, overlap);

	}
}



